#include <HWLib.h>
#include "Include/mcp2515.h"
#include "Include/ArduinoMcp2515Defs.h"

#define MCP_RESET	 0xC0
#define MCP_READ	  0x03
#define MCP_WRITE	 0x02

#define CANSTAT	   0x0E
#define CANCTRL	   0x0F
#define BFPCTRL	   0x0C
#define CNF3		0x28
#define CNF2		0x29
#define CNF1		0x2A
#define CANINTF		0x2c

#define CS_ON	IOPut(p4, OFF)
#define CS_OFF	IOPut(p4, ON)

void MCPWriteRegister(unsigned char adress, unsigned char value);
unsigned char MCPReadRegister(unsigned char adress);
void SendPacket();

typedef struct
{
	unsigned char txbsidh;	//TRANSMIT BUFFER n STANDARD IDENTIFIER HIGH
	unsigned char txbsidl;	//TRANSMIT BUFFER n STANDARD IDENTIFIER LOW
	unsigned char txbeid8;	//TXBnEID8 – TRANSMIT BUFFER n EXTENDED IDENTIFIER HIGH
	unsigned char txbeid0;	//TXBnEID0 – TRANSMIT BUFFER n EXTENDED IDENTIFIER LOW
	unsigned char txbdlc;	//TRANSMIT BUFFER n DATA LENGTH CODE
	unsigned char data[8];
	
}MCPPacket;

unsigned char SPIRW(unsigned char b)
{
	SPI2BUF = b;
	while(!(SPI2STAT&0b1));
	unsigned char r = SPI2BUF;
	return r;
}

void MCPSendPacket(CanPacket packet)
{
	SendPacket();
}



int MCPReceivePacket(CanPacket *packet)
{
	MCPWriteRegister(0x1d, 0); //reset errors
	
	unsigned char canintf = MCPReadRegister(CANINTF);
	if(!(canintf&0x01))
		return 0;
		
	MCPPacket mcppacket;
	unsigned char i;
	for(i=0; i<sizeof(MCPPacket); i++)
	{
		((unsigned char*)&mcppacket)[i] = MCPReadRegister(0x61 + i);
	}
	
	packet->numData = mcppacket.txbdlc;
	
	for(i=0; i<8; i++)
		packet->data[i] = mcppacket.data[i];
		
	MCPWriteRegister(CANINTF, canintf&0b11111110);
	
	return 1;
}

void SendPacket()
{
	MCPPacket packet;
	packet.txbsidh = 0x0;
	packet.txbsidl = 0;
	packet.txbeid8 = 0;
	packet.txbeid0 = 0;
	packet.txbdlc = 8;
	
	packet.data[0] = 0x12;
	packet.data[1] = 0x23;
	packet.data[2] = 0x45;
	packet.data[3] = 0x67;
	packet.data[4] = 0x89;
	packet.data[5] = 0xAB;
	packet.data[6] = 0xCD;
	packet.data[7] = 0xEF;
	
	unsigned char i;
	for(i=0; i<sizeof(MCPPacket); i++)
	{
		MCPWriteRegister(0x31 + i, ((unsigned char*)&packet)[i]);
	}
	
	MCPWriteRegister(0x30, 0b00001011); //request transmission, highest priority
}

void MCPWriteRegister(unsigned char adress, unsigned char value)
{
	CS_ON;
	//DelayMs(10);
	//SPI2BUF = MCP_WRITE;
	SPIRW(MCP_WRITE);
	//DelayMs(10);
	//SPI2BUF = adress;
	SPIRW(adress);
	//DelayMs(10);
	//SPI2BUF = value;
	SPIRW(value);
	//DelayMs(10);
	CS_OFF;
	//DelayMs(10);
}

unsigned char MCPReadRegister(unsigned char adress)
{
	unsigned char r;
	//SPI2STAT = 0b1000000000000000;
	
	CS_ON;
	//DelayMs(10);
		//SPI2STAT = 0b1000000000000000;
	//SPI2BUF = MCP_READ;
	SPIRW(MCP_READ);
	//DelayMs(10);
	//mu = SPI2BUF;
		//SPI2STAT = 0b1000000000000000;
	//SPI2BUF = adress;
	SPIRW(adress);
	//DelayMs(10);
	//mu = SPI2BUF;
	r = SPIRW(0);
	//SPI2STAT = 0b1000000000000000;
	//SPI2BUF = 0x0;
	//DelayMs(10);
	
	//DelayMs(10);
	//mu = SPI2BUF;
	CS_OFF;
	//DelayMs(1);
	return r;
}

void SPIInit()
{
/*        RPOR1bits.RP2R = 11;    // Assign RP2 to SCK2 (output)
        RPOR1bits.RP3R = 10;    // Assign RP3 to SDO2 (output)
        RPINR22bits.SDI2R = 11; // Assign RP11 to SDI2 (input)

        XYZ_CS_IO = 0;          // CS SPI enable low (1 I2C)
//      XYZ_VDD_IO = 1;         // VDD to acc
//      XYZ_VDD_TRIS = 0;       // Make the VDD pin an output
        XYZ_CS_TRIS = 0;        // Make the CS pin an output
        XYZ_RDY_TRIS = 1;       // Make the RDY/INT pin an input
        XYZ_SDI_TRIS = 1;       // Make the DIN pin an input
        XYZ_SDO_TRIS = 0;       // Make the DOUT pin an output
        XYZ_SCK_TRIS = 0;       // Make the SCK pin an output
*/
	IOInit(p8, SPICLKOUT);
	IOInit(p10, SPI_OUT);
	IOInit(p12, SPI_IN);
	//IOInit(12, IN);
	IOInit(p4, OUT);
	CS_OFF;
		
  //TRISF = 0x0080;
  //CLKDIV = 0x0040;            
	//SPI2CON1 = 0x013B;          
  //SPI2CON2 = 0x0000;
  //SPI2STAT = 0x8000;    
	
	//movlw   0x40        ;   Mode 1,1 SPI, middle of output 
	//movwf   SSPSTAT     ;   time sampling
//OpenSPI2(0x0120 , 0x0000, 0x8000);

	//SPI2STAT = 0b0000000000000000;
	SPI2CON1 = 0b0000000000100000;
	SPI2CON2 = 0b0000000000000000;
	
	SPI2STAT = /*SPI2STAT |*/ 0b1000000000000000;
	
	
	        #define SPI_MASTER      (1u<<5)
        #define SPI_SPRE_1_1    (0x07<<2)
        #define SPI_SPRE_2_1    (0x06<<2)
        #define SPI_SPRE_3_1    (0x05<<2)
        #define SPI_SPRE_4_1    (0x04<<2)
        #define SPI_SPRE_5_1    (0x03<<2)
        #define SPI_SPRE_6_1    (0x02<<2)
        #define SPI_SPRE_7_1    (0x01<<2)
        #define SPI_SPRE_8_1    (0x00<<2)
        #define SPI_PPRE_1_1    (0x03)
        #define SPI_PPRE_4_1    (0x02)
        #define SPI_PPRE_16_1   (0x01)
        #define SPI_PPRE_64_1   (0x00)
        
        #define SPI_DISABLE     0x0000  // diable SPI port
        #define SPI_ENABLE      0x8000  // enable SPI port, clear status
		/*
        SPI2STAT = SPI_DISABLE;      // Turn off SPI
        SPI2CON1 = (SPI_MASTER | SPI_PPRE_1_1 | SPI_SPRE_2_1); // Fcy Primary prescaler 8:1, secondary prescaler 1:1, CPK=1, CKE=0, SMP=1
        SPI2CON2 = 0x0000;
        SPI2STAT = SPI_ENABLE;       // Turn on SPI
		*/
	//SPI1CON1 = 0x013B;          
	//SPI1CON2 = 0x0000;
	//SPI1STAT = 0x8000;  
}

void MCPInit()
{
	SPIInit();
	
	CS_ON;
	//DelayMs(5);
	//SPI2BUF = MCP_RESET;
	//DelayMs(5);
	SPIRW(MCP_RESET);
	CS_OFF;
	
	//MCSWriteRegister(CANSTAT, );
	//MCSWriteRegister(CANCTRL, );
	//MCSWriteRegister(BFPCTRL, );
	
	//MCPWriteRegister(CNF1, 0b01000001);
	//MCPWriteRegister(CNF2, 0b11111010);
	//MCPWriteRegister(CNF3, 0b00000110);

	//MCPWriteRegister(CNF1, 0b01000001);
	//MCPWriteRegister(CNF1, 0b01000011);  //125khz
	
	/*MCPWriteRegister(CNF1, 0b11001001);  //50khz
	MCPWriteRegister(CNF2, 0b11111010);
	MCPWriteRegister(CNF3, 0b00000110);*/
	//MCPWriteRegister(CNF1, 0x07);  //50khz
	//MCPWriteRegister(CNF2, 0xb4);
	//MCPWriteRegister(CNF3, 0x04);
	
	MCPWriteRegister(0x2B, 0b00000000);  //CANINTE no interrupts
	MCPWriteRegister(0x0F, 0b01100000);  //CANCTRL listen only mode
	//MCPWriteRegister(0x0F, 0b00001000);  //CANCTRL normal mode, one shot
	//MCPWriteRegister(0x0F, 0b01010000);  //CANCTRL loopback mode

	MCPWriteRegister(0x0C, 0b00000000);  //BFPCTRL controlpins unused
	MCPWriteRegister(0x0D, 0b00000000);  //TXRTSCTRL controlpins unused
	
	MCPWriteRegister(0x60, 0b01100000);  //RXB0CTRL

	MCPWriteRegister(0x20, 0b00000000);  //mask0 high
	MCPWriteRegister(0x21, 0b00000000);  //mask0 low
	MCPWriteRegister(0x23, 0b00000000);  //mask0 low
}


int AutoBaud(int CAN_Bus_Speed, byte Freq, byte SJW) {
  if(SJW < 1) SJW = 1;
  if(SJW > 4) SJW = 4;
  if(CAN_Bus_Speed>0) {
    if(Can_init(CAN_Bus_Speed, Freq, SJW, FALSE)) return CAN_Bus_Speed;
  } else {
      int i=0;
      byte interruptFlags = 0;
      for(i=5; i<1000; i=i+5) {
		  sprintf(buf,"Testing Baudrate: %i \r\n",i);
		  UARTWrite(1,buf);
        if(Can_init(i, Freq, SJW, TRUE)) {
            // check for bus activity
			UARTWrite(1,"Bus Activity Found!!\r\n");
            MCPWriteRegister(CANINTF,0);
            DelayMs(500); // need the bus to be communicating within this time frame
            if(MCPReadRegister(CANINTF)) {
              // determine which interrupt flags have been set
              interruptFlags = MCPReadRegister(CANINTF);
              if(!(interruptFlags & MERRF)) {
                // to get here we must have received something without errors
                //Mode(MODE_NORMAL);
				
                  return i;
              }
            }
        }
      }
  }
  return 0;
}

BOOL Can_init(int CAN_Bus_Speed, byte Freq, byte SJW, BOOL autoBaud) {
  
  // Reset MCP2515 which puts it in configuration mode
	CS_ON;
	SPIRW(MCP_RESET);
	CS_OFF;
  
  // Calculate bit timing registers
  byte BRP;
  float TQ;
  byte BT;
  float tempBT;

  float NBT = 1.0 / (float)CAN_Bus_Speed * 1000.0; // Nominal Bit Time
  for(BRP=0;BRP<8;BRP++) {
    TQ = 2.0 * (float)(BRP + 1) / (float)Freq;
    tempBT = NBT / TQ;
      if(tempBT<=25) {
        BT = (int)tempBT;
        if(tempBT-BT==0) break;
      }
  }
  
  byte SPT = (0.7 * BT); // Sample point
  byte PRSEG = (SPT - 1) / 2;
  byte PHSEG1 = SPT - PRSEG - 1;
  byte PHSEG2 = BT - PHSEG1 - PRSEG - 1;


  // Programming requirements
  if(PRSEG + PHSEG1 < PHSEG2) return FALSE;
  if(PHSEG2 <= SJW) return FALSE;
  
  byte BTLMODE = 1;
  byte SAM = 0;
  
  // Set registers
  byte data = (((SJW-1) << 6) | BRP);
  MCPWriteRegister(CNF1, data);
  MCPWriteRegister(CNF2, ((BTLMODE << 7) | (SAM << 6) | ((PHSEG1-1) << 3) | (PRSEG-1)));
  MCPWriteRegister(CNF3, (0b10000000 | (PHSEG2-1)));
  MCPWriteRegister(TXRTSCTRL,0);
  
  /*if(!autoBaud) {
    // Return to Normal mode
      if(!Mode(MODE_NORMAL)) return FALSE;
  } else {
    // Set to Listen Only mode
      if(!Mode(MODE_LISTEN)) return FALSE;
  }
  */
  // Enable all interupts
  MCPWriteRegister(CANINTE,255);
  
  // Test that we can read back from the MCP2515 what we wrote to it
  byte rtn = MCPReadRegister(CNF1);
  return (rtn==data);
}